package com.dizner.androidcartest;

import android.os.SystemClock;

import com.google.android.things.pio.I2cDevice;
import com.google.android.things.pio.PeripheralManager;
import com.google.android.things.userdriver.pio.I2cBusDriver;

import java.io.IOException;

@Deprecated
public class MotorHAL {

    I2cDevice i2cDevice;

//    public MotorHAL() {

//    }

    public static final int FORWARD = 1;
    public static final int BACKWARD = 2;
    public static final int BRAKE = 3;
    public static final int RELEASE = 4;
    public static final int SINGLE = 1;
    public static final int DOUBLE = 2;
    public static final int INTERLEAVE = 3;
    public static final int MICROSTEP = 4;

    private int addr;
    private int freq;

    private PWM _pwm;

    private Adafruit_DCMotor[] motors;

    public MotorHAL() {
        PeripheralManager pm = PeripheralManager.getInstance();
        try {
            i2cDevice = pm.openI2cDevice("I2C1", 0x60);
        } catch (IOException e) {
            e.printStackTrace();
        }
        this.addr = 0x60;
        this.freq = 1600;
        _pwm = new PWM();
        _pwm.setPWMFreq(freq);
        motors = new Adafruit_DCMotor[]{new Adafruit_DCMotor(this, 0), new Adafruit_DCMotor(this, 1), new Adafruit_DCMotor(this, 2), new Adafruit_DCMotor(this, 3)};
    }

    private void setPin(int pin, int value) {
        if ((pin < 0) || (pin > 15)) {
            throw new IllegalArgumentException("PWM pin must be between 0 and 15 inclusive");
        }
        if ((value != 0) && (value != 1)) {
            throw new IllegalArgumentException("Pin value must be 0 or 1!");
        }
        if (value == 0) {
            _pwm.setPWM(pin, 0, 4096);
        }
        if (value == 1) {
            _pwm.setPWM(pin, 4096, 0);
        }
    }


    public Adafruit_DCMotor getMotor(int num) {
        if ((num < 1) || (num > 4)) {
            throw new IllegalArgumentException("MotorHAT Motor must be between 1 and 4 inclusive");
        }
        return motors[num - 1];
    }


    static class Adafruit_DCMotor {
        private int pwm = 0, in1 = 0, in2 = 0;

        private MotorHAL MC;

        public Adafruit_DCMotor(MotorHAL controller, int num) {
            this.MC = controller;
            if (num == 0) {
                pwm = 8;
                in2 = 9;
                in1 = 10;
            } else if (num == 1) {
                pwm = 13;
                in2 = 12;
                in1 = 11;
            } else if (num == 2) {
                pwm = 2;
                in2 = 3;
                in1 = 4;
            } else if (num == 3) {
                pwm = 7;
                in2 = 6;
                in1 = 5;
            } else {
                throw new IllegalArgumentException("MotorHAT Motor must be between 1 and 4 inclusive");
            }
        }

        public void run(int command) {
            if (MC == null) {
                return;
            }
            if (command == MotorHAL.FORWARD) {
                MC.setPin(in2, 0);
                MC.setPin(in1, 1);
            }
            if (command == MotorHAL.BACKWARD) {
                MC.setPin(in1, 0);
                MC.setPin(in2, 1);
            }
            if (command == MotorHAL.RELEASE) {
                MC.setPin(in1, 0);
                MC.setPin(in2, 0);
            }
        }
    }

    class PWM {
        //Registers/etc.
        public static final int __MODE1 = 0x00;
        public static final int __MODE2 = 0x01;
        public static final int __SUBADR1 = 0x02;
        public static final int __SUBADR2 = 0x03;
        public static final int __SUBADR3 = 0x04;
        public static final int __PRESCALE = 0xFE;
        public static final int __LED0_ON_L = 0x06;
        public static final int __LED0_ON_H = 0x07;
        public static final int __LED0_OFF_L = 0x08;
        public static final int __LED0_OFF_H = 0x09;
        public static final int __ALL_LED_ON_L = 0xFA;
        public static final int __ALL_LED_ON_H = 0xFB;
        public static final int __ALL_LED_OFF_L = 0xFC;
        public static final int __ALL_LED_OFF_H = 0xFD;
        // Bits
        public static final int __RESTART = 0x80;
        public static final int __SLEEP = 0x10;
        public static final int __ALLCALL = 0x01;
        public static final int __INVRT = 0x10;
        public static final int __OUTDRV = 0x04;

        private int address = 0x40;

        public PWM() {


            setAllPWM(0, 0);
            try {
                i2cDevice.writeRegByte(__MODE2, (byte) __OUTDRV);
                i2cDevice.writeRegByte(__MODE1, (byte) __ALLCALL);
                SystemClock.sleep((long) 0.005); //wait for oscillator

                byte mode1 = i2cDevice.readRegByte(__MODE1);
                mode1 = (byte) (mode1 & ~__SLEEP);                //wake up (reset sleep)
                i2cDevice.writeRegByte(__MODE1, mode1);
                SystemClock.sleep((long) 0.005);
            } catch (IOException e) {
                e.printStackTrace();
            }
        }

        public void setPWMFreq(int freq) {
            double prescaleval = 25000000.0;    // 25MHz
            prescaleval /= 4096.0;     // 12-bit
            prescaleval /= freq;
            prescaleval -= 1.0;
//            if (self.debug) {
//
//                print "Setting PWM frequency to %d Hz" % freq
//                print "Estimated pre-scale: %d" % prescaleval
//            }
            double prescale = Math.floor(prescaleval + 0.5);
            try {
                byte oldmode = i2cDevice.readRegByte(__MODE1);
                int newmode = (oldmode & 0x7F) | 0x10;            // sleep
                i2cDevice.writeRegByte(__MODE1, (byte) newmode);      // go to sleep
                i2cDevice.writeRegByte(__PRESCALE, (byte) Math.floor(prescale));
                i2cDevice.writeRegByte(__MODE1, oldmode);
                SystemClock.sleep((long) 0.005);
                i2cDevice.writeRegByte(__MODE1, (byte) (oldmode | 0x80));
            } catch (IOException e) {
                e.printStackTrace();
            }
//            if (self.debug) {
//
//                print "Final pre-scale: %d" % prescale
//            }
        }


        public void setPWM(int channel, int on, int off) {
            try {
                i2cDevice.writeRegByte(__LED0_ON_L + 4 * channel, (byte) (on & 0xFF));
                i2cDevice.writeRegByte(__LED0_ON_H + 4 * channel, (byte) (on >> 8));
                i2cDevice.writeRegByte(__LED0_OFF_L + 4 * channel, (byte) (off & 0xFF));
                i2cDevice.writeRegByte(__LED0_OFF_H + 4 * channel, (byte) (off >> 8));
            } catch (IOException e) {
                e.printStackTrace();
            }
        }

        public void setAllPWM(int on, int off) {
            try {
                i2cDevice.writeRegByte(__ALL_LED_ON_L, (byte) (on & 0xFF));
                i2cDevice.writeRegByte(__ALL_LED_ON_H, (byte) (on >> 8));
                i2cDevice.writeRegByte(__ALL_LED_OFF_L, (byte) (off & 0xFF));
                i2cDevice.writeRegByte(__ALL_LED_OFF_H, (byte) (off >> 8));
            } catch (IOException e) {
                e.printStackTrace();
            }
        }

    }

//    static class Adafruit_MotorHAT {
//
//    }
}
